//
// Created by bill3000 on 3/4/21.
//
#include <unistd.h>

#include "fruit.hpp"
#include "../log/logger.hpp"
#include "../timer/timer.hpp"

int FruitFSM::start_sorting_thread(int size_and_sugar) {
    int ret;
    pthread_t id;
    pthread_attr_t thread_attr;

    ret = pthread_attr_init(&thread_attr);
    std::string ss = "Failed to start sorting thread !";
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED);
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_create(&id, &thread_attr, sorting_thread, (void *)&size_and_sugar);
    if(ret!=0) {
        LOG(ERROR, ss);
        return -1;
    }
    pthread_attr_destroy(&thread_attr);

    return 0;
}

void * FruitFSM::sorting_thread(void *data) {
    int size_and_sugar = *(int *)data;

    action_start_conveyor(nullptr);
    sorting = true;
    addon_sugar_thread_id = -1;
    statistics = {0};

    if (size_and_sugar == 1) {
        sorting_by_size();
    }
    else if (size_and_sugar == 2) {
        sorting_by_sugar();
    }
    else {
        start_the_addon_sugar_thread();
        sorting_by_size_and_sugar();
    }

    //Notify FSM we finished learning
    Message msg;
    msg.mtype = 300;
    msg.data.ctype = EVENT_LEARNING_SAL_COMPLETED;
    mq->send_message(M02, &msg);

    util_delete_dir_content("/intelli/working/");
    action_stop_conveyor(nullptr);
    pthread_exit(nullptr);
}

void FruitFSM::sorting_by_size(int andbysugar) {
    cv::Mat m1, m2, m3;

    Event ev;
    ev.extra.cam = loaded_size_model[0].cam;
    action_modify_cam_param(&ev);
    ev.extra.cam = loaded_size_model[1].cam;
    action_modify_cam_param(&ev);
    ev.extra.cam = loaded_size_model[2].cam;
    action_modify_cam_param(&ev);

    int begin = 0, end = 0;
    while(sorting) {
        usleep(50000); //delay 50ms

        *cam1 >> m1; *cam2 >> m2; *cam3 >> m3;
        if (m1.empty() || m2.empty() || m3.empty()) {
            LOG(ERROR, "Capture image error !");
            sorting = false;
            break;
        }
        int area_threshold = m1.rows * m1.cols / 9;

        int raw_area = algorithm_get_image_area(m1);
        int area = 0;
        if (raw_area > area_threshold)
            area = raw_area;

        if (area == 0) {
            if (begin == 0) {  //a new round of start
                begin = 1;
                continue;
            }
            if (end == 1) {
                //Here, we got a perfect series of a fruit images
                //We must get the one in the middle of the series
                //And analyze this fruit's size and defect
                //The following function's return, -1:defect, 1:large, 2:middle, 3:small
                int size = algorithm_analysis_size_and_defect();
                if (andbysugar == 0) {
                    TimerNode tn;
                    tn.to = 2; //机电控制逻辑线程, 定时结果消息进入消息队列M3;
                    tn.howlong = 3000; //delay 3000ms
                    tn.dowhat = 03; //0x01，启动皮带；0x02，停止皮带；0x03，推动推杆；0x04，复位机电系统
                    tn.param = size;  //皮带编号/推杆编号
                    OkTimer::add_timer(tn);

                    if (size == 1) statistics.large[0]++;
                    else if (size == 2) statistics.middle[0]++;
                    else if (size == 3) statistics.small[0]++;
                    else statistics.defective++;
                } else {
                    //Now, we get the size catalog in "size", we should notify sugar thread to start analyzing
                    Message m;
                    m.mtype = 1000;
                    m.data.ctype = 0x01;
                    m.data.param = size;
                    mq->send_message(M04, &m);
                }

                begin = 0;  //start a new round
                end = 0;
            }
            continue;
        }
        if (begin == 0) continue;

        //We get a fruit image and put it in the "/intelli/tracking/size/" directory
        end = 1;
        std::string fn = util_get_now_time_milli_string();
        fn.insert(0, "/intelli/tracking/size/");
        std::string fn1 = fn, fn2 = fn, fn3 = fn;
        fn1.append("-01.jpg"); fn2.append("-02.jpg"); fn3.append("-03.jpg");
        cv::imwrite(fn1, m1); cv::imwrite(fn2, m2); cv::imwrite(fn3, m3);
    }
    util_delete_dir_content("/intelli/tracking/size/");
}

void FruitFSM::sorting_by_size_and_sugar() {
    sorting_by_size(1);  //0: no sugar, 1: including sugar analysis
}

void FruitFSM::sorting_by_sugar() {
    cv::Mat m4;

    Event ev;
    ev.extra.cam = loaded_size_model[0].cam;
    action_modify_cam_param(&ev);

    int begin = 0, end = 0;
    while(sorting) {
        usleep(50000); //delay 50ms

        *cam4 >> m4;
        if (m4.empty()) {
            LOG(ERROR, "Capture image error !");
            sorting = false;
            break;
        }
        int area_threshold = m4.rows * m4.cols / 9;

        int raw_area = algorithm_get_image_area(m4);
        int area = 0;
        if (raw_area > area_threshold)
            area = raw_area;

        if (area == 0) {
            if (begin == 0) {  //a new round of start
                begin = 1;
                continue;
            }
            if (end == 1) {
                //Here, we got a perfect series of a fruit images
                //We must get the one in the middle of the series
                //And analyze this fruit's size and defect
                //The following function's return, 1:sweet, 2:not sweet
                int sugar = algorithm_analysis_sugar();

                TimerNode tn;
                tn.to = 2; //机电控制逻辑线程, 定时结果消息进入消息队列M3;
                tn.howlong = 1500; //delay 3000ms
                tn.dowhat = 03; //0x01，启动皮带；0x02，停止皮带；0x03，推动推杆；0x04，复位机电系统
                tn.param = sugar;  //皮带编号/推杆编号
                OkTimer::add_timer(tn);

                if (sugar == 1) statistics.sweet++;
                else statistics.notsweet++;

                begin = 0;  //start a new round
                end = 0;
            }
            continue;
        }
        if (begin == 0) continue;

        //We get a fruit image and put it in the "/intelli/tracking/size/" directory
        end = 1;
        std::string fn = util_get_now_time_milli_string();
        fn.insert(0, "/intelli/tracking/sugar/").append(".jpg");
        cv::imwrite(fn, m4);
    }
    util_delete_dir_content("/intelli/tracking/sugar/");
}

int FruitFSM::start_the_addon_sugar_thread() {
    int ret;
    pthread_attr_t thread_attr;

    ret = pthread_attr_init(&thread_attr);
    std::string ss = "Failed to start addon sugar thread !";
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED);
    if (ret != 0) {
        LOG(ERROR, ss);
        return -1;
    }
    ret = pthread_create(&addon_sugar_thread_id, &thread_attr, addon_sugar_thread, (void *) nullptr);
    if(ret!=0) {
        LOG(ERROR, ss);
        return -1;
    }
    pthread_attr_destroy(&thread_attr);

    return 0;
}

void* FruitFSM::addon_sugar_thread(void *) {
    Message m;

    while(sorting) {
        int ret = mq->receive_message(M04, &m, 1000); //m.mtype = 1000; m.data.ctype = 0x01; m.data.param = size;
        if (ret == -1) {
            LOG(ERROR, strerror(errno));
            continue;
        }
        int size = m.data.param;

        cv::Mat m4;
        Event ev;
        ev.extra.cam = loaded_size_model[0].cam;
        action_modify_cam_param(&ev);

        int begin = 0, end = 0;
        bool goon = true;
        while(goon) {
            usleep(50000); //delay 50ms

            *cam4 >> m4;
            if (m4.empty()) {
                LOG(ERROR, "Capture image error !");
                sorting = false;
                break;
            }
            int area_threshold = m4.rows * m4.cols / 9;

            int raw_area = algorithm_get_image_area(m4);
            int area = 0;
            if (raw_area > area_threshold)
                area = raw_area;

            if (area == 0) {
                if (begin == 0) {  //a new round of start
                    begin = 1;
                    continue;
                }
                if (end == 1) {
                    //Here, we got a perfect series of a fruit images
                    //We must get the one in the middle of the series
                    //And analyze this fruit's size and defect
                    //The following function's return, 1:sweat, 2:not sweat
                    int sugar = algorithm_analysis_sugar();

                    if (size == 1) {
                        if (sugar == 1) statistics.large[0]++;
                        else statistics.large[1]++;
                    } else if (size == 2) {
                        if (sugar == 1) statistics.middle[0]++;
                        else statistics.middle[1]++;
                    } else {
                        if (sugar == 1) statistics.small[0]++;
                        else statistics.small[1]++;
                    }

                    TimerNode tn;
                    tn.to = 2; //机电控制逻辑线程, 定时结果消息进入消息队列M3;
                    tn.howlong = 1500; //delay 3000ms
                    tn.dowhat = 03; //0x01，启动皮带；0x02，停止皮带；0x03，推动推杆；0x04，复位机电系统
                    tn.param = sugar;  //皮带编号/推杆编号
                    OkTimer::add_timer(tn);

                    goon = false;
                }
                continue;
            }
            if (begin == 0) continue;

            //We get a fruit image and put it in the "/intelli/tracking/size/" directory
            end = 1;
            std::string fn = util_get_now_time_milli_string();
            fn.insert(0, "/intelli/tracking/sugar/").append(".jpg");
            cv::imwrite(fn, m4);
        }
        util_delete_dir_content("/intelli/tracking/sugar/");
    }
    pthread_exit(nullptr);
}

